/**
 * @file Dog.h
 * @brief
 * @author Luke (AiNuozhou@bupt.edu.cn)
 * @version 1.0
 * @date 2021-10-27
 *
 * @copyright Copyright (c) 2021  北京邮电大学
 *
 * @par 修改日志:
 * @date 2021-10-27
 * @author Luke (AiNuozhou@bupt.edu.cn)
 */

#ifndef DOG_H
#define DOG_H
#include "oslib.h"
#include "servo.h"
#include "utils.h"

#define LEG_A1 0.11
#define LEG_B1 0.042
#define LEG_L1 0.025
#define LEG_L2 0.073
#define LEG_L3 0.093

#define TAIJIE_PARM 22
// #define LEG_A1 0.110
// #define LEG_L1 0.0263 + 0.01363
// #define LEG_L2 0.09625
// #define LEG_L3 0.15075 - 0.016

#define GAIT_S 0.035 //前进距离
#define GAIT_H 0.035 //抬腿高度
#define GAIT_T 1     //每个步态的时间周期
#define GAIT_DS 0.03 //后蹬距离
#define GAIT_R 1
#define GAIT_DTH 0.1745 // 10 * PI / 180; //一个周期实现的转向角
#define GAIT_dth 0.0524 // 3 * PI / 180;  //一个周期实现的转弯角
#define GAIT_DZ 0.0265 // sqrt(0.220^2-0.060^2)/8;  %%要重新算，或以实际值为准

#define INIT_POS 2048
#define INIT_ANGLE 0

#define LEG0_INIT_X 0
// #define LEG0_INIT_Y 0.1395
#define LEG0_INIT_Y 0.165
#define LEG0_INIT_Z 0.105

#define LEG1_INIT_X 0
#define LEG1_INIT_Y 0.165
#define LEG1_INIT_Z 0.105

#define LEG2_INIT_X 0
#define LEG2_INIT_Y 0.165
#define LEG2_INIT_Z 0.105

#define LEG3_INIT_X 0
#define LEG3_INIT_Y 0.165
#define LEG3_INIT_Z 0.105

// dog adjust 0.035 0.05 4 0 0 0.01 0.12 0
typedef enum {
  DOG_MODE_NONE = 0,
  DOG_MODE_STOP,
  DOG_MODE_INIT,
  DOG_MODE_RUN,
  DOG_MODE_TURN11, //左转向
  DOG_MODE_TURN21, //右转向
  DOG_MODE_TURN12,
  DOG_MODE_TURN22,
  DOG_MODE_LEFT,
  DOG_MODE_RIGHT,
  DOG_MODE_DOWN,
  DOG_MODE_UPSTAIR, //上楼梯用这个，调一调参数
  DOG_MODE_SQUARE,  //方形步态，用来过减速带
  DOG_MODE_DEBUG,
} DogMode_e;

typedef struct {
  uint8_t No;
  Servo_s servo[3];
  double foot_x;
  double foot_y;
  double foot_z;

} Leg_s;

typedef struct {
  float v_x;
  float v_y;
  float v_w;

  float pos_x;
  float pos_y;
  float pos_z;

  float yaw;
  float pitch;
  float roll;

  uint8_t upstair_flag;

  Leg_s legs[4];
  DogMode_e ctrl_mode;
} Dog_s;

extern Dog_s Dog;
extern Time_s Dog_time;

extern float delta_h;
extern float delta_s;
extern float delta_t;
extern float delta_ds;
extern float delta_dth;
extern float delta_Dth;
extern float delta_dz;
extern float dz_k;
extern float delta_dh;

extern float barycenter_front;
extern float barycenter_back;
extern float barycenter_left;
extern float barycenter_right;
extern float barycenter_central;

void Dog_init();
void Dog_exe();
void Dog_Gait_Init();
void Dog_Gait_Reset();
void Dog_Gait_centroid_forward();
void Dog_Gait_1();
void Dog_Gait_2();
void Dog_Gait_3();
void Dog_Gait_zhuanxiang1_1();
void Dog_Gait_zhuanxiang1_2();
void Dog_Gait_zhuanxiang2_1();
void Dog_Gait_zhuanxiang2_2();
void Dog_Gait_zuozhuanwan();
void Dog_Gait_youzhuanwan();
void Dog_Gait_xiapo();
void Do_Gait_shangtaijie();
void Do_Gait_fangxingbutai();
void Dog_Gait_zhongxinzuoyi();
void Dog_Gait_shangtaijie_zuoyou();
void Dog_Gait_turn1_1();
void Dog_Gait_turn1_2();
void Dog_Gait_turn2_1();
void Dog_Gait_turn2_2();
void Dog_Gait_upstairs1();
void Dog_Gait_upstairs();
void Dog_Inverse_kinematics(Leg_s *leg);
void Dog_servo_angle_pos();
void Dog_servo_pos_angle();
#endif // DOG_H